Estimating Probability of Collision for Safe Planning under Gaussian Motion and Sensing Uncertainty

We present a fast, analytical method for estimating the probability of collision of a motion plan for a mobile robot operating under the assumptions of Gaussian motion and sensing uncertainty. Estimating the probability of collision is an integral step in many algorithms for motion planning under uncertainty and is crucial for characterizing the safety of motion plans. Our method computes an accurate, yet conservative, estimate of the probability of collision based on a priori distributions of the robot state along a given plan. We specifically account for the fact that the probabilities of collision at each stage along the plan are conditioned on the previous stages being collision free, by using a novel method to truncate the a priori state distributions with respect to obstacles and correctly propagating them forward in time. Our method can be directly applied within a variety of existing motion planners to improve their performance and the quality of computed plans. We apply our method to a car-like mobile robot with second order dynamics and to a steerable medical needle in 3D and demonstrate that our method for estimating the probability of collision is orders of magnitude faster than naı̈ve Monte-Carlo sampling methods and reduces estimation error by more than 25% compared to prior methods.